#include "utils.h"

double getContourLength(const std::vector<cv::Point> &contour) {
    double length = 0.0;
    for (size_t i = 0; i < contour.size(); ++i) {
        cv::Point start = contour[i];
        cv::Point end = contour[(i + 1) % contour.size()];
        length += cv::norm(end - start);
    }
    return length;
}

std::vector<cv::Point> computePolygonFromMask(const cv::Mat &mask) {
    // Pad the mask with a border of 1 pixel
    cv::Mat paddedMask;
    int padWidth = 1;
    cv::copyMakeBorder(mask, paddedMask, padWidth, padWidth, padWidth, padWidth, cv::BORDER_CONSTANT, 0);

    // Find contours
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(paddedMask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    if (contours.empty()) {
        std::cerr << "No contour found, so returning empty polygon." << std::endl;
        return std::vector<cv::Point>();
    }

    // Find the contour with the maximum length
    auto maxContourIt = std::max_element(contours.begin(), contours.end(),
                                         [](const std::vector<cv::Point>& a, const std::vector<cv::Point>& b) {
                                             return cv::arcLength(a, false) < cv::arcLength(b, false);
                                         });
    std::vector<cv::Point> contour = *maxContourIt;

    // Adjust contour coordinates due to padding
    for (auto& point : contour) {
        point -= cv::Point(padWidth, padWidth);
    }

    // Approximate the polygon
    const double POLYGON_APPROX_TOLERANCE = 0.004;
    cv::Rect boundingBox = cv::boundingRect(contour);
    double maxDim = std::max(boundingBox.width, boundingBox.height);
    double epsilon = POLYGON_APPROX_TOLERANCE * maxDim;
    std::vector<cv::Point> polygon;
    cv::approxPolyDP(contour, polygon, epsilon, true);

    // Clip the polygon to image boundaries
    for (auto& point : polygon) {
        point.x = std::clamp(point.x, 0, mask.cols - 1);
        point.y = std::clamp(point.y, 0, mask.rows - 1);
    }

    // Remove the last point if it is a duplicate of the first point
    if (!polygon.empty() && polygon.front() == polygon.back()) {
        polygon.pop_back();
    }

    return polygon;
}
